//
// Created by chen on 25-5-10.
//

#include "ControlLoop.h"
#include <cmath>
#define polePairs 7
ControlLoop::ControlLoop(float kp, float ki, float integralLimit, float outputLimit, float dcBusVoltage)
    : piController(kp, ki, integralLimit, outputLimit), svpwm(dcBusVoltage),
      valpha(0.0f), vbeta(0.0f), targetValue(0.0f), actualValue(0.0f), outputD(0.0f), outputQ(0.0f) {}

void ControlLoop::setTarget(float target) {
    targetValue = target;
}

void ControlLoop::setActualValue(float value) {
    actualValue = value;
}

void ControlLoop::setParkInverseTransform(float vd, float vq) {
    outputD = vd;
    outputQ = vq;
}

void ControlLoop::execute(float angle) {
    // 根据机械角度和极对数计算电角度
    float electricalAngle = angle * polePairs;

    // 执行PI控制（输出为q轴电流指令）
    float output = piController.compute(targetValue, actualValue);

    // 最大转矩控制：d轴为0，q轴为PI输出（保留注释部分的逻辑）
    outputD = 0.0f;
    outputQ = output;

    // 使用电角度执行反Park变换（将dq轴电压转换为αβ轴电压）
    float cosTheta = std::cos(electricalAngle);
    float sinTheta = std::sin(electricalAngle);
    valpha = outputD * cosTheta - outputQ * sinTheta;
    vbeta = outputD * sinTheta + outputQ * cosTheta;
}

void ControlLoop::getPwmDutyCycle(float angle,float& dutyU, float& dutyV, float& dutyW) {
    svpwm.modulate(valpha, vbeta, angle,dutyU, dutyV, dutyW);
}

void ControlLoop::reset() {
    piController.reset();
}